/humanoid/desiredBaseTrajecory
rviz display
/humanoid/currentState
/humanoid_sqp_mpc
/nodelet_manager
/humanoid_hand_control
/humanoid_quest_control_with_arm
/state_estimate/Contact_Detection/mode
/humanoid_controller/swing_leg/pos_measured
Type: std_msgs/Float64MultiArray
作用待验证
/humanoid_sqp_mpc
/humanoid_joy_control_auto_gait_with_vel
/humanoid/mpc/terrainHeght
Type: std_msgs/Float64
/hand_wrench/left_hand
左手力矩数据
/humanoid/optimizedStateTrajectory
/humanoid/planedFootPositions
rviz display
/hand_wrench/right_hand
右手力矩数据
/humanoid_VR_hand_control
/humanoid_ee_State
Type: kuavo_msgs/endEffectorData
/humanoid_hand_control
humanoid_controller
humanoid_common
/humanoid_single_step_control
type:kuavo_msgs/singleStepControl
footPoseTargetTrajectories
单步控制,通过给出时间序列和对应的躯干位姿,可以控制机器人的单步行走
时间序列和躯干位姿序列长度必须一致,时间序列需要不断递增
每次服务请求的躯干位姿都是基于局部坐标系,但是一次服务请求中的躯干位姿序列需要以第一个位姿为基准不断变化
/gesture/list
列出所有预设的手势
/gesture/execute
获取当前控制模式,返回 control_mode
/gesture/execute_state
获取当前控制模式,返回 control_mode
/humanoid_joy_control_auto_gait_with_vel
/nodelet_manager
/humanoid_change_arm_ctrl_mode
受srv控制,需要是external_control_mode
服务端:/humanoid_sqp_mpc
/kuavo_arm_target_poses
type:kuavo_msgs/armTargetPoses
控制机器人手臂在指定时间内到达目标位置
/humanoid_VR_hand_control
/humanoid_Arm_time_target_control
/humanoid_get_arm_ctrl_mode
获取当前控制模式,返回 control_mode
/humanoid_mpc_mode_scale
type: std_msgs/Float32
步态的缩放比例,用于控制步频topic
/humanoid_mpc_stop_step_num
type: std_msgs/Int32
停止步数,从当前统计的步数开始,机器人会在后续第N步自动停下
/cmd_vel
type: geometry_msgs/Twist
控制指令,6dof 速度指令,机器人的target指令的速度形式,包含xy方向速度、高度z和yaw方向速度,但 roll、pitch 方向不控制
/humanoid_mpc_target
type:ocs2_msgs/mpc_target_trajectories
发送给mpc的期望状态topic
/humanoid_mpc_target_arm
type:ocs2_msgs/mpc_target_trajectories
发送给mpc的期望状态topic
/humanoid_mpc_target_pose
type:ocs2_msgs/mpc_target_trajectories
发送给mpc的期望躯干状态6dof 位姿规划指令
注意位姿指令优先级比 cmd_vel 指令高,不要同时发送两种指令
/humanoid_hand_control
/humanoid_quest_control_with_arm
/joystick_node
这是一个伪节点,可以是遥控也可以是terminal脚本
/humanoid_mpc_foot_pose_target_trajectories
type: kuavo_msgs/footPoseTargetTrajectories
用于单步控制,指定脚的步态位姿,以及对应的躯干姿态。
/humanoid_mpc_mode_schedule
type:ocs2_msgs/mode_schedule
步态序列切换topic
/cmd_pose
type: geometry_msgs/Twist
位置控制指令, 可用与控制机器人从当前位置到达目标 pose
/humanoid_sqp_mpc
关键算法和技术指标:
/humanoid_auto_gait
srv:是否自动切换gait
/joy
Type: sensor_msgs/Joy
/humanoid_joy_control_auto_gait_with_vel
收集启动信息脚本分析: 记录和管理ROS启动环境信息的工具,对于异常排查和系统监控(sh脚本)
收集启动环境信息:获取Git版本控制信息、时间戳、机器人序列号等
创建唯一的信息存储目录:基于父进程ID创建唯一的目录
管理历史记录:限制存储的启动记录和coredump数量
记录崩溃环境:保存可能发生崩溃的环境信息
robot_version_management 机器人版本管理以及rviz可视化配置(launch)
配置mpc控制器可视化参数
list中有可选取步态:{[0] stance, [1] trot, [2] jump, [3] sl ; single leg stance, [4] walk, [5] walk2, [6] trot2}
walk
{
modeSequence
{
[0] SS
[1] ST
[2] SF
[3] SH
[4] SS
[5] TS
[6] FS
[7] HS
}
switchingTimes
{
[0] 0.05
[1] 0.07
[2] 0.35
[3] 0.04
[4] 0.05
[5] 0.07
[6] 0.35
[7] 0.04
}
}
back_walk
{
modeSequence
{
[0] SS
[1] SS
[2] SF
[3] ST
[4] SS
[5] SS
[6] FS
[7] TS
}
switchingTimes
{
[0] 0.05
[1] 0.07
[2] 0.35
[3] 0.04
[4] 0.05
[5] 0.07
[6] 0.35
[7] 0.04
}
}
kalmanFilter:
启动humanoidsqpmpc节点(node),允许在节点前启动脚本 start_node.sh
/control_robot_hand_position
type: kuavo_msgs/robotHandPosition
只有在kuavo.json中配置EndEffectorType为qiangnao或qiangnao_touch时才会发布该话题.
话题描述: 用于控制机器人双手(手指)的运动,通过发布手指目标关节位置来实现手部的精确控制。
/dexhand/command
type: kuavo_msgs/dexhandCommand
只有在kuavo.json中配置EndEffectorType为qiangnao_touch时才会发布该话题.
话题描述: 用于控制机器人双手(手指)的运动,通过发布手指目标关节位置来实现手部的精确控制。
<?xml version="1.0" ?>
<launch>
<arg name="rviz" default="false" />
<arg name="description_name" default="humanoid_description"/>
<arg name="robot_version" default="$(optenv ROBOT_VERSION 40)"/> 默认从环境变量ROBOT_VERSION 读取,如果不存在设为40
<param name="robot_version" value="$(arg robot_version)"/>
<!-- <arg name="urdfFile" default="$(find biped_s3)/urdf/biped_s3_with_v3_arm.urdf"/> -->
<arg name="urdfFile" default="$(find kuavo_assets)/models/biped_s$(arg robot_version)/urdf/biped_s$(arg robot_version).urdf"/>
`urdfFile`: 机器人的URDF模型文件路径,基于版本号动态生成
`description_name`: ROS参数服务器中存储URDF的参数名
<!-- params -->
laucn中进行参数设置直接引入工程代码
- `taskFile`: MPC 和 WBC 各任务和约束,主要的修改文件;
- `referenceFile`: 用于调整遥控器的限幅,和各关节的控制模式和上层扭矩的kpkd;
- `dynamicQrFile`: 动态权重配置,MPC 根据步态,切换到对应的QR矩阵;
- `gaitFile`: 步态命令配置,定义不同行走模式,以及步态相位顺序和对应时间
- `kuavoConfigFile`: 机器人JSON配置文件,包含关键硬件参数可视化参数
- `rviz`: 是否启动RViz可视化工具,默认为false
- `rvizconfig`: RViz配置文件路径
<param name="urdfFile" value="$(arg urdfFile)" />
<param name="taskFile" value="$(find humanoid_controllers)/config/kuavo_v$(arg robot_version)/mpc/task.info"/>
<param name="referenceFile" value="$(find humanoid_controllers)/config/kuavo_v$(arg robot_version)/command/reference.info"/>
<param name="dynamicQrFile" value="$(find humanoid_controllers)/config/kuavo_v$(arg robot_version)/mpc/dynamic_qr.info"/>
<param name="gaitCommandFile" value="$(find humanoid_controllers)/config/kuavo_v$(arg robot_version)/command/gait.info"/>
<arg name="kuavoConfigFile" default="$(find kuavo_assets)/config/kuavo_v$(arg robot_version)/kuavo.json"/>
<param name="kuavo_configuration" textfile="$(arg kuavoConfigFile)"/>
<param name="modelPath" value="$(find kuavo_assets)/models" />
<!-- rviz -->
<!-- <group if="$(arg rviz)"> -->
<param name="$(arg description_name)" textfile="$(arg urdfFile)"/>
<arg name="rvizconfig" default="$(find humanoid_interface_ros)/rviz/humanoid.rviz" />
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rvizconfig)" output="screen" />
<!-- </group> -->
</launch>
启动mujoco仿真 nodelet_with_arm launch文件
启动rosbag_nodelet节点
启动ros_joytick节点
启动系统信息发布节点
启动kuavo状态监控节点
启动ros nodelet manager 为镜像配置参数为
启动加载一个 actual controller implementation (named nodelet_controller (controller 节点)) 到nodelet manager中
允许在节点前启动脚本 start_node.sh
Humanoid-control
humanoid_interface_ros
qpoases_catkin
/dexhand/left/command
type: kuavo_msgs/dexhandCommand
只有在kuavo.json中配置EndEffectorType为qiangnao_touch时才会发布该话题.
话题描述: 用于控制机器人双手(手指)的运动,通过发布手指目标关节位置来实现手部的精确控制。
/leju_claw_command
type: kuavo_msgs/lejuCalwCommand
只有在kuavo.json中配置EndEffectorType为lejuclaw时才会发布该话题.
话题描述: 话题描述: 该话题用于控制二指夹爪
注意/警告:如果当前夹爪处于运动状态,那么发送新的请求就会被丢弃处理,不会执行,望知悉。
/dexhand/right/command
type: kuavo_msgs/dexhandCommand
只有在kuavo.json中配置EndEffectorType为qiangnao_touch时才会发布该话题.
话题描述: 用于控制机器人双手(手指)的运动,通过发布手指目标关节位置来实现手部的精确控制。
dexhand/touch_state
type: kuavo_msgs/dexhandTouchState
话题描述: 发布触觉灵巧手的触觉状态数据.
/control_robot_leju_claw
type: kuavo_msgs/controlLejuClaw
只有在kuavo.json中配置EndEffectorType为lejuclaw时才会发布该话题.
该服务用于控制二指夹爪
/leju_claw_state
type: kuavo_msgs/lejuClawState
话题描述: 发布二指夹抓的状态, 位置, 速度, 力距等信息
/humanoid_gait_command
/humanoid_gait_switch_by_name
/humanoid_controller/wbc_planned_body_acc/linear
type:std_msgs/Float64MultiArray
wbc优化后的躯干线性加速度,, 单位(rad/s)
ROS_simulate_mujoco_startup_Sequence
一些重要的参数指标
wbc frequency 500hz
sensor frequency 500hz
arm_move_spd 3.0
#include <ros/init.h>
#include <ros/package.h>
#include <ocs2_sqp/SqpMpc.h>
#include <humanoid_interface/HumanoidInterface.h>
#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h>
#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
#include <ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h>
#include "humanoid_interface_ros/gait/GaitReceiver.h"
#include "ocs2_core/thread_support/SetThreadPriority.h"
using namespace ocs2;
using namespace humanoid;
int main(int argc, char **argv)
{
const std::string robotName = "humanoid";
// Initialize ros node
::ros::init(argc, argv, robotName + "_mpc_sqp");
::ros::NodeHandle nodeHandle;
// Get node parameters
bool multiplot = false;
std::string taskFile, urdfFile, referenceFile,gaitCommandFile;
nodeHandle.getParam("/multiplot", multiplot);
nodeHandle.getParam("/taskFile", taskFile);
nodeHandle.getParam("/referenceFile", referenceFile);
nodeHandle.getParam("/urdfFile", urdfFile);
nodeHandle.getParam("/gaitCommandFile", gaitCommandFile);
int version_num;
if (nodeHandle.hasParam("/robot_version"))
{
nodeHandle.getParam("/robot_version", version_num);
}
// Robot interface
HumanoidInterface interface(taskFile, urdfFile, referenceFile, gaitCommandFile, version_num);
interface.setupCPUconfig();// 配置内核隔离和线程优先级
// Gait receiver
auto gaitReceiverPtr =
std::make_shared<GaitReceiver>(nodeHandle, interface.getSwitchedModelReferenceManagerPtr(), robotName);
// ROS ReferenceManager
auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(robotName, interface.getReferenceManagerPtr());
rosReferenceManagerPtr->subscribe(nodeHandle);
// MPC
SqpMpc mpc(interface.mpcSettings(), interface.sqpSettings(), interface.getOptimalControlProblem(),
interface.getInitializer());
mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);
// Launch MPC ROS node
MPC_ROS_Interface mpcNode(mpc, robotName);
mpcNode.launchNodes(nodeHandle);
std::cout << "!!!!!!!!!!!!!!!!!!!end!!!!!!!!!!!!!!" << std::endl;
// Successful exit
return 0;
}
<launch>
<arg name="legged_robot_scene" default="$(find kuavo_assets)/models/biped_s$(optenv ROBOT_VERSION 40)/xml/scene.xml"/>
<param name="legged_robot_scene_param" value="$(arg legged_robot_scene)" />
<node pkg="nodelet" type="nodelet" name="nodelet_mujoco" args="load MujocoNodelet nodelet_manager" respawn="false" output="screen" />
</launch>
/humanoid_VR_hand_control
/nodelet_manager
/robot_head_motion_data
type:kuavo_msgs/robotHeadMotionData
用于控制机器人头部的运动,通过发布目标关节角度来实现头部控制,位控。
/类似于手臂重映射,编码器命令直接下发
/kuavo_arm_traj
type:sensor_msgs/JointState
用于控制机器人手臂运动,通过发布手臂目标关节位置来实现手臂的精确控制,目前位控.
/nodelet_manager
/humanoid_joy_control_auto_gait_with_vel
/humanoid_change_arm_ctrl_mode
受srv控制,需要是外部控制?
/humanoidcontroller/swing_leg/vel_measured*
type:std_msgs/Float64MultiArray
发布接触点当前的测量速度, 单位(m/s)
/humanoid_controller/wbc_planned_body_acc/angular
type:std_msgs/Float64MultiArray
wbc优化后的躯干角加速度, 单位(rad/s)
hardware_interface
Demo/graspBox
/humanoid_joy_control_auto_gait_with_vel
/humanoid_foot_contact_point
type:std_msgs/Float32MultiArray
/humanoid_sqp_mpc
/humanoid_mpc_arm_commanded
type:ocs2_msgs/mpc_target_trajectories
发布当前使用的手臂目标轨迹
/humanoid_mpc_gait_time_name
type: kuavo_msgs/gaitTimeName
步态的时间和名字topic
dexhand/state
type: sensor_msgs/JointState
话题描述: 发布灵巧手的状态数据.
/enable_wbc_arm_trajectory_control
/humanoidcontroller/swing_leg/pos_measured*
type:std_msgs/Float64MultiArray
发布接触点当前的测量位置, 单位(m) 根据状态估计发布的质心位置,结合前向运动学发布
/humanoid_joy_control_auto_gait_with_vel
/humanoid_mpc_policy
Type: ocs2_msgs/mpc_flattened_controller
mpc计算的结果
/nodelet_manager
/humanoid_sqp_mpc
/humanoid_controller/com/com_lf_diff
type:std_msgs/Float64MultiArray
左脚质心位置 xyz, 单位(m)
/humanoid_controller/com/com_lf_diff_des
type:std_msgs/Float64MultiArray
左脚质心期望位置 xyz, 单位(m)
/humanoid_controller/com/com_rf_diff
type:std_msgs/Float64MultiArray
右脚质心位置 xyz, 单位(m)
/humanoid_controller/com/rd_base
type:std_msgs/Float64MultiArray
质心期望相对base坐标系的位置
/humanoid_controller/swing_leg/vel_desired
type:std_msgs/Float64MultiArray
数组长度24, 即左右接触点(0~7) xyz 的期望速度
发布接触点的期望速度, 单位(m/s)
/humanoid_controller/swing_leg/vel_measured
type:std_msgs/Float64MultiArray
发布接触点当前的实际测量速度速度, 单位(m/s)
初始化控制器,- Initializes the controller with hardware interface, node handle, and nodelet flag
/humanoid_controller/com/rd_des
type:std_msgs/Float64MultiArray
质心期望速度
/humanoid_controller/com/r_des
type:std_msgs/Float64MultiArray
质心期望位置
hardware_interface
gazebo
/humanoid_controller/com/r
type:std_msgs/Float64MultiArray
质心位置
/humanoid_controller/feet_target_policys
/humanoid_controller/com/rd
type:std_msgs/Float64MultiArray
质心速度
/enable_mpc_flag
控制 MPC 启用/禁用
/enable_wbc_flag
控制 WBC 启用/禁用
/humanoid_controller/swing_leg/pos_measured
type:std_msgs/Float64MultiArray
发布接触点当前的测量位置, 单位(m) 根据状态估计发布的质心位置,结合前向运动学发布
/hand_wrench_cmd
手部力矩命令
加载控制器稳定数据,传感器数据流_preUpdate
wbc内部变量 logger发布
mujoco_nodelet
/sensors_data_raw
type:kuavo_msgs/sensorsData
实物机器人, 仿真器发布的传感器原始数据
/joint_cmd
type:kuavo_msgs/jointCmd
关节命令下发
/bot_stand_up_complete
控制器主循环
使能控制器_starting
1.设置机器人参数配置,包含版本号,关节数,初始高度,传感器种类等;
2.根据info文件设置机器人的配置参数;
3.初始化共享内存
4.初始化gait manager,默认启动步态stance
5.初始化ddp_mpc
6.初始化mrt_mpc
7.初始化pinocchio可视化
8.初始化硬件接口等
9.初始化状态估计等
选择控制器类型
nodelet_Controller
state_estimation callback function
/sensors_data_raw
controller_nodelet
/start_mpc
/humanoid_controller/optimizedState_mrt/com/angular_vel_xyz
type:std_msgs/Float64MultiArray
从mpc(mrt)取得的质心线速度, 顺序为xyz, 单位(m/s)
/humanoid_controller/swing_leg/acc_desired
type:std_msgs/Float64
发布接触点期望加速度, 单位( m/s2 )
/humanoid_controller/optimizedState_mrt/joint_pos
type:std_msgs/Float64MultiArray
从mpc(mrt)取得的关节位置, 单位(radian)
/stateestimate/end_effector/contact_point*/feet_height
估计的第x个接触点的"足端高度"
/state_estimate/base/angular_vel_zyx
估计的躯干角速度,顺序为zyx
/state_estimate/base/linear_vel
估计的躯干线速度,顺序为xyz
/state_estimate/base/angular_zyx
估计的躯干线速度,顺序为xyz(ypr)
/humanoidcontroller/swing_leg/acc_desired*
type:std_msgs/Float64MultiArray
发布特定序号的接触点期望加速度, 单位(m/s2)
humanoid_controller/optimizedState_mrt/base/pos_xyz
type:std_msgs/Float64MultiArray
从mpc(mrt)取得的躯干位置, 单位(m)
/humanoid_controller/optimizedState_mrt/base/angular_zyx
type:std_msgs/Float64MultiArray
从mpc(mrt)取得的躯干角速度, 顺序为zyx, 单位(radian/s)
/humanoid_controller/optimizedState_mrt/com/angular_zyx
type:std_msgs/Float64MultiArray
从mpc(mrt)取得的质心角速度, 顺序为zyx, 单位(radian/s)
/stateestimate/end_effector/contact_point*/pos
第x个接触点的位置
/humanoidcontroller/optimizedInput_mrt/force*
type:std_msgs/Float64MultiArray
从mpc(mrt)取得的第x个接触点的接触力, 单位(N)
/state_estimate/mode
估计的步态mode
/state_estimate/joint/pos
估计的关节位置
SensorDataPublisher
/joint_states
编码器原始数据
/imu
imu原始数据
/humanoid_mpc_observation
Type: ocs2_msgs/mpc_observation
观测值包含当前状态,mpc端接收
mpc_observation是简化手部自由度的,只用在mpc内部
/humanoid_controller/com/com_rf_diff_des
type:std_msgs/Float64MultiArray
右脚质心期望位置 xyz, 单位(m)
/humanoid_controller/optimizedInput_mrt/joint_vel
type:std_msgs/Float64MultiArray
从mpc(mrt)取得的关节期望速度
/humanoid_controller/optimized_mode
type:std_msgs/Float64
mpc 给出的 mode
/state_estimate/base/pos_xyz
估计的躯干位置,顺序为xyz
/humanoid_VR_hand_control
/humanoid_wbc_observation
Type: ocs2_msgs/mpc_observation
观测值包含当前状态,mpc端接收
wbc_observation是全自由度,包含手部自由度
/humanoid_Arm_time_target_control